#include "JPQuat.h"

void JPQuat::setAxisAngle(const JPVec3 &axis, float angle)
{
	s = cosf(angle/2);
	JPVec3 v = axis*sinf(angle/2);
	x = v.x; y = v.y; z= v.z;
}
void JPQuat::getAxisAngle(JPVec3 &axis, float &angle) const
{
	angle = acosf(s);

	const float invSin = 1.0f/sinf(angle);

	axis.x = x*invSin;
	axis.y = y*invSin;
	axis.z = z*invSin;

	// multiply by 2
	angle*=2.0f;
}

void JPQuat::conjugate()
{
	x=-x;y=-y;z=-z;
}

void JPQuat::invert()
{
	conjugate();
	*this/=sqrLength(); 
}

void JPQuat::normalise()
{
	*this/=length();
}

JPQuat JPQuat::getNormalised() const
{
	return *this/length();
}

JPVec3 JPQuat::rotateVec(const JPVec3 &v) const
{
	JPQuat qv(v.x,v.y,v.z,0);
	JPQuat conjugate(*this);
	conjugate.conjugate();

	JPQuat r = *this * qv* conjugate;
	return JPVec3(r.x,r.y,r.z);
}

float JPQuat::length() const
{
	return sqrtf(x*x+y*y+z*z+s*s);
}

float JPQuat::sqrLength() const
{
	return (x*x+y*y+z*z+s*s);
}